/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 *
 * $Id: ASRoboBus.cpp,v 1.5 2009-05-13 07:18:13 amaula Exp $
 *
 * \file
 * \brief ASRobo Compatible BUS baseclass implementation.
 * \author Antti Maula <antti.maula@tkk.fi>
 */
#include "ASRoboBus.hpp"
#include "owndebug.h"
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

CASRoboBus::CASRoboBus() 
  : iMutexHandle( ownMutex_Init() ),
    iCondHandle( ownCond_Init() ),
    iBusMode(0),
    iIsBusOpen(false)
{
}
//*****************************************************************************

CASRoboBus::~CASRoboBus()
{
  ownCond_Destroy(iCondHandle);
  ownMutex_Destroy(iMutexHandle);
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

int CASRoboBus::ReadBlock(unsigned char *aBuffer, int aBlock_size, 
		    int aTimeout_ms)
{
  int r,bytes_read = 0;
  EBusStatus status;
  int evil_sleep_ms = 0;
  const unsigned int timestamp_begin = ownTime_get_ms();

  // Attempt to run until whole buffer filled. (This can be interrupted on
  // timeout or error)
  while(bytes_read < aBlock_size) {

    // Check whether the Timeout value is used
    if (aTimeout_ms >= 0) {
      // WaitToRead for current amount of aTimeout
      status = WaitToRead(aTimeout_ms);
      if (status == KBusStatusOK || status == KBusStatusUnimplemented) {
	// OK And Unimplemented get to here. Unimplemented causes a increasing Sleep()
	if (status == KBusStatusUnimplemented) {
	  dPrint(6,"WARNING: WaitToWrite() returned 'Unimplemented'. Read() Can block!");
	}

	// Status == OK == We got data. Safe to call Read()
	r = Read(aBuffer+bytes_read, aBlock_size-bytes_read);

	// Check for Read() operation status
	if (r > 0) {
	  // Append the number of bytes read to pointer.
	  bytes_read += r;

	  // Report status
	  dPrint(20,"Got %d bytes on this read, totals %d bytes; Total required is %d bytes.",
		 r, bytes_read, aBlock_size);

	} else if (r == 0) {
	  // The Read returned zero, which means that nothing got read
	  // (NonBlocking socket?)
	  dPrint(3,"WARNING: Read() returned Zero! Non-Blocking socket? Forcing 10ms sleep.");
	  ownSleep_ms(10);
	  
	} else {
	  // Read returned <= 0
	  dPrint(8,"Error from Read()! This will abort the whole ReadBlock()");
	  bytes_read = -1;
	  break;
	}
      } 
      
      // Check for any timeout condition (new timeout calculated in any case)
      aTimeout_ms -= ownTime_get_ms_since(timestamp_begin);
      if (status != KBusStatusOK || aTimeout_ms <= 0) {
	dPrint(8,"Timeout while reading %d bytes buffer. Got %d bytes so far.",
	       aBlock_size, bytes_read);
	break;
      }
    } else {
      // Timeout_ms was already under 0, so wait forever requested.
      r = Read(aBuffer+bytes_read, aBlock_size-bytes_read);
      if (r > 0) {
	bytes_read += r;
	dPrint(20,"Got %d bytes on this read, totals %d bytes; Total required is %d bytes.",
	       r, bytes_read, aBlock_size);

      } else if (r == 0) {
	// The Read returned zero, which means that nothing got written
	// (NonBlocking socket?)
	dPrint(3,"WARNING: Read() returned Zero! Non-Blocking socket? Sleep! (%d ms)",
	       evil_sleep_ms);
	ownSleep_ms(evil_sleep_ms);
	if (evil_sleep_ms < 1000) evil_sleep_ms += 20;
      } else {
	dPrint(8,"Error from Read()! This will abort the whole ReadBlock()");
	bytes_read = -1;
	break;
      }
    }
  }
  
  // Just dbg
  dPrint(15,"ReadBlock complete, final result is bytes_read = %d",
	 bytes_read);

  // Return the number of bytes read, or error value.
  return bytes_read;
}
//*****************************************************************************

int CASRoboBus::WriteBlock(const unsigned char *aBuffer, int aBlock_size, 
		     int aTimeout_ms)
{
  int r,bytes_written = 0;
  int evil_sleep_ms = 0;
  EBusStatus status;
  const unsigned int timestamp_begin = ownTime_get_ms();

  // Attempt to run until whole buffer written. (This can be interrupted on
  // timeout or error)
  while(bytes_written < aBlock_size) {

    // Check whether the Timeout value is used
    if (aTimeout_ms >= 0) {
      // WaitToWrite for current amount of aTimeout
      status = WaitToWrite(aTimeout_ms);
      if (status == KBusStatusOK || status == KBusStatusUnimplemented) {

	// OK And Unimplemented get to here. Unimplemented causes a increasing Sleep()
	if (status == KBusStatusUnimplemented) {
	  dPrint(6,"WARNING: WaitToWrite() returned 'Unimplemented'. Write() can block!");
	}

	// Status == OK == We got data. Safe to call Read()
	r = Write(aBuffer+bytes_written, aBlock_size-bytes_written);

	// Check for () operation status
	if (r > 0) {
	  // Append the number of bytes read to pointer.
	  bytes_written += r;

	  // Report status
	  dPrint(20,"Wrote %d bytes on this write, totals %d bytes; Total required is %d bytes.",
		 r, bytes_written, aBlock_size);

	} else if (r == 0) {
	  // The Write returned zero, which means that nothing got written
	  // (NonBlocking socket?)
	  dPrint(3,"WARNING: Write() returned Zero! Non-Blocking socket? Forcing 10ms sleep.");
	  ownSleep_ms(10);
	  
	} else {
	  // Write returned <= 0
	  dPrint(8,"Error from Write()! This will abort the whole WriteBlock()");
	  bytes_written = -1;
	  break;
	}
      } else {
	dPrint(8,"WaitToWrite() returned Error! Abort.");
	bytes_written = -1;
	break;

      } 
      
      // Check for any timeout condition (new timeout calculated in any case)
      aTimeout_ms -= ownTime_get_ms_since(timestamp_begin);
      if (status == KBusStatusTimeout || aTimeout_ms <= 0) {
	dPrint(8,"Timeout while writing %d bytes buffer. Wrote %d bytes so far.",
	       aBlock_size, bytes_written);
	break;
      }
    } else {
      // Timeout_ms was already under 0, so wait forever requested.
      r = Write(aBuffer+bytes_written, aBlock_size-bytes_written);
      if (r > 0) {
	bytes_written += r;
	dPrint(20,"Wrote %d bytes on this write, totals %d bytes; Total required is %d bytes.",
	       r, bytes_written, aBlock_size);

      } else if (r == 0) {
	// The Write returned zero, which means that nothing got written
	// (NonBlocking socket?)
	dPrint(3,"WARNING: Write() returned Zero! Non-Blocking socket? Forcing sleep! (%d ms)", 
	       evil_sleep_ms);
	ownSleep_ms(evil_sleep_ms);
	if (evil_sleep_ms < 1000) evil_sleep_ms += 20;
	
      } else {
	dPrint(8,"Error from Write()! This will abort the whole WriteBlock()");
	bytes_written = -1;
	break;
      }
    }
  }
  
  // Just dbg
  dPrint(15,"WriteBlock complete, final result is bytes_written = %d",
	 bytes_written);

  // Return the number of bytes written
  return bytes_written; 
}
//*****************************************************************************

bool CASRoboBus::SetBusMode(EBusModeBit aBusModeBits) {
  iBusMode |= aBusModeBits;
  return true;
}
//*****************************************************************************

bool CASRoboBus::ClearBusMode(EBusModeBit aBusModeBits) {
  iBusMode &= ~aBusModeBits;
  return true;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

void CASRoboBus::Lock(void)
{
  ownMutex_Lock(iMutexHandle);
}
//*****************************************************************************

void CASRoboBus::Unlock(void)
{
  ownMutex_Unlock(iMutexHandle);
}
//*****************************************************************************

void CASRoboBus::Notify()
{
  ownCond_Broadcast(iCondHandle);
}
//*****************************************************************************

bool CASRoboBus::Wait(const int aTimeout_ms)
{
  bool result = false;
  ownStatus s = ownCond_Wait(iCondHandle, 
			     iMutexHandle, 
			     aTimeout_ms >= 0 ? aTimeout_ms : -1);
  if (s == ownStatusOK) result = true;
  return result;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
